#include "sign_vec.h"
#include <base/common.h>
#include <base/utility.h>
#include <base/fileUtils.h>
#include <base/macro.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <iostream>

using namespace ville3d;

SignVec::SignVec(const std::string& img_path) : rayLength_(50.0), image_root_(img_path)
{

}

SignVec::~SignVec()
{

}

std::vector<Eigen::Vector3d> SignVec::DoWork1(SignBatch& signBatch, std::map<std::string, Pose>& poseMap, const Camera& camera)
{
    std::vector<SignVecMeta> valid_batches;
    for (SignVecMeta vec : signBatch.SignVecs)
    {
        if (poseMap.find(vec.imagename) != poseMap.end())
        {
            vec.pose = poseMap[vec.imagename];
            valid_batches.push_back(vec);
        }
    }

    if (valid_batches.size() < 3)
    {
        std::cout << "the valid batches is too small to vectorizalize" << std::endl;
        return std::vector<Eigen::Vector3d>();
    }

    undistortbBox(valid_batches, camera);
    Eigen::Matrix3d cRb = camera.mount.rotation.matrix();
    Eigen::Vector3d cTb = camera.mount.translation;

    int N = valid_batches.size();
    envelope2D bBox = valid_batches[0].dlSign.bBox;
    Eigen::Vector2d min_env, max_env;
    bBox.GetRange(min_env, max_env);

    std::vector<Eigen::Vector2d> q1(4), q2(4);
    q1[0] = Eigen::Vector2d(min_env.x(), min_env.y());
    q1[1] = Eigen::Vector2d(max_env.x(), min_env.y());
    q1[2] = Eigen::Vector2d(max_env.x(), max_env.y());
    q1[3] = Eigen::Vector2d(min_env.x(), max_env.y());

    Eigen::Matrix3d bRw0 = valid_batches[0].pose.orientation.toRotationMatrix();
    Eigen::Vector3d bTw0 = valid_batches[0].pose.position;
    Eigen::Matrix3d R0 = bRw0 * cRb;
    Eigen::Vector3d C0 = bTw0 + bRw0 * cTb;

    double depth = 50.0;
    std::vector<Eigen::Vector3d> Points3d(4), Ray(4);
    for (int i = 0; i < 4; i++)
    {
        Ray[i] = R0 * camera.K.inverse() * Eigen::Vector3d(q1[i].x(), q1[i].y(), 1.0);
        Ray[i].normalize();
        Points3d[i] = C0 + depth *  Ray[i];
    }

    double stopThresholdLM = 0.0;
    double currentEro = 0.0;
    double currentLambda = 0.0;
    double ni = 2.0;

    //Generate init lamda
	{
        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 1);
        for(int i = 1; i < N; i++)
        {
            envelope2D bBox = valid_batches[i].dlSign.bBox;
            Eigen::Vector2d min_env, max_env;
            bBox.GetRange(min_env, max_env);

            q2[0] = Eigen::Vector2d(min_env.x(), min_env.y());
            q2[1] = Eigen::Vector2d(max_env.x(), min_env.y());
            q2[2] = Eigen::Vector2d(max_env.x(), max_env.y());
            q2[3] = Eigen::Vector2d(min_env.x(), max_env.y());

            Eigen::Matrix3d bRw = valid_batches[i].pose.orientation.toRotationMatrix();
            Eigen::Vector3d bTw = valid_batches[i].pose.position;
            Eigen::Matrix3d R = bRw * cRb;
            Eigen::Vector3d C = bTw + bRw * cTb;

            for (int j = 0; j < 4; j++)
            {
                Eigen::Vector3d camera3D = R.transpose() * (Points3d[j] - C);
                if (camera3D[2] <= 0.0)
                    continue;

                Eigen::Vector3d uvw = camera.K * camera3D;
                Eigen::Vector2d uv(uvw[0] / uvw[2], uvw[1] / uvw[2]);
                Eigen::Vector2d residual = uv - q2[j];
                currentEro += residual.transpose() * residual;

                Eigen::Matrix<double, 2, 3> J_F;
                J_F << camera.fx() / camera3D.z(), 0.0, -camera.fx() * camera3D.x() / (camera3D.z() * camera3D.z()),
                    0.0, camera.fy() / camera3D.z(), -camera.fy() * camera3D.y() / (camera3D.z() * camera3D.z());
                Eigen::Matrix<double, 2, 1> J_aux = J_F * R.transpose() * Ray[j];
			    H += J_aux.transpose() * J_aux;
            }
		}

        stopThresholdLM = 1e-6 * currentEro;
        double maxDiagonal = std::max(std::fabs(H(0, 0)), maxDiagonal);
        double tau = 1e-5;
        currentLambda = tau * maxDiagonal;
    }

    auto robustWeightCauchy = [=](double norm_res) -> double {
		return std::sqrt(1.0 / (1.0 + norm_res * norm_res));
	};

	int iter = 0;
	bool stop = false;
	while (!stop && iter < 30)
	{
		bool oneStepSuccess = false;
        int false_cnt = 0;

		while (!oneStepSuccess)
		{
            Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 1);
            Eigen::MatrixXd g = Eigen::MatrixXd::Zero(1, 1);

            for (int i = 1; i < N; i++)
            {
                Points3d[0] = C0 + depth * Ray[0];
                Points3d[1] = C0 + depth * Ray[1];
                Points3d[2] = C0 + depth * Ray[2];
                Points3d[3] = C0 + depth * Ray[3];

                envelope2D bBox = valid_batches[i].dlSign.bBox;
                Eigen::Vector2d min_env, max_env;
                bBox.GetRange(min_env, max_env);

                q2[0] = Eigen::Vector2d(min_env.x(), min_env.y());
                q2[1] = Eigen::Vector2d(max_env.x(), min_env.y());
                q2[2] = Eigen::Vector2d(max_env.x(), max_env.y());
                q2[3] = Eigen::Vector2d(min_env.x(), max_env.y());

                /*q2[0] = Eigen::Vector2d(valid_batches[i].dlSign.bBox.x, valid_batches[i].dlSign.bBox.y);
                q2[1] = Eigen::Vector2d(valid_batches[i].dlSign.bBox.x + valid_batches[i].dlSign.bBox.width, valid_batches[i].dlSign.bBox.y);
                q2[2] = Eigen::Vector2d(valid_batches[i].dlSign.bBox.x + valid_batches[i].dlSign.bBox.width, valid_batches[i].dlSign.bBox.y + valid_batches[i].dlSign.bBox.height);
                q2[3] = Eigen::Vector2d(valid_batches[i].dlSign.bBox.x, valid_batches[i].dlSign.bBox.y + valid_batches[i].dlSign.bBox.height);
                */

                Eigen::Matrix3d bRw = valid_batches[i].pose.orientation.toRotationMatrix();
                Eigen::Vector3d bTw = valid_batches[i].pose.position;
                Eigen::Matrix3d R = bRw * cRb;
                Eigen::Vector3d C = bTw + bRw * cTb;

                for (int j = 0; j < 4; j++)
                {
                    Eigen::Vector3d camera3D = R.transpose() * (Points3d[j] - C);
                    if (camera3D[2] <= 0.0)
                        continue;

                    Eigen::Vector3d uvw = camera.K * camera3D;
                    Eigen::Vector2d uv(uvw[0] / uvw[2], uvw[1] / uvw[2]);
                    Eigen::Vector2d residual = uv - q2[j];
                    
                    double err_i_norm = residual.norm();
				    double weight = robustWeightCauchy(err_i_norm);

                    Eigen::Matrix<double, 2, 3> J_F;
                    J_F << camera.fx() / camera3D.z(), 0.0, -camera.fx() * camera3D.x() / (camera3D.z() * camera3D.z()),
                        0.0, camera.fy() / camera3D.z(), -camera.fy() * camera3D.y() / (camera3D.z() * camera3D.z());
                    Eigen::Matrix<double, 2, 1> J_aux = J_F * R.transpose() * Ray[j];
                    
                    H += J_aux.transpose() * J_aux * weight;
                    g -= J_aux.transpose() * residual * weight;
                }
            }

            H(0, 0) += currentLambda;

			Eigen::Matrix<double, 1, 1> delta = H.colPivHouseholderQr().solve(g);
            if (delta.squaredNorm() < EPS || false_cnt > 10)
            {
                stop = true;
                break;
            }

            /*for (int i = 0; i < 4; i++)
            {
                Points3d[i] = C0 + depth * Ray[i];
            }
            signBatch.geometry3d = Points3d;
            signBatch.SignVecs.swap(valid_batches);
            Dump(signBatch, camera);*/

            depth += delta(0, 0);

            /*for (int i = 0; i < 4; i++)
            {
                Points3d[i] = C0 + depth * Ray[i];
            }

            signBatch.geometry3d = Points3d;
            Dump(signBatch, camera);*/

            double scale = 0;
            scale = delta.transpose() * (currentLambda * delta + g);
            scale += 1e-3;

			double temp_err = 0.0;
            for (int i = 1; i < N; i++)
            {
                Points3d[0] = C0 + depth * Ray[0];
                Points3d[1] = C0 + depth * Ray[1];
                Points3d[2] = C0 + depth * Ray[2];
                Points3d[3] = C0 + depth * Ray[3];

                envelope2D bBox = valid_batches[i].dlSign.bBox;
                Eigen::Vector2d min_env, max_env;
                bBox.GetRange(min_env, max_env);

                q2[0] = Eigen::Vector2d(min_env.x(), min_env.y());
                q2[1] = Eigen::Vector2d(max_env.x(), min_env.y());
                q2[2] = Eigen::Vector2d(max_env.x(), max_env.y());
                q2[3] = Eigen::Vector2d(min_env.x(), max_env.y());

                Eigen::Matrix3d bRw = valid_batches[i].pose.orientation.toRotationMatrix();
                Eigen::Vector3d bTw = valid_batches[i].pose.position;
                Eigen::Matrix3d R = bRw * cRb;
                Eigen::Vector3d C = bTw + bRw * cTb;

                for (int j = 0; j < 4; j++)
                {
                    Eigen::Vector3d camera3D = R.transpose() * (Points3d[j] - C);
                    if (camera3D[2] <= 0.0)
                        continue;

                    Eigen::Vector3d uvw = camera.K * camera3D;
                    Eigen::Vector2d uv(uvw[0] / uvw[2], uvw[1] / uvw[2]);
                    Eigen::Vector2d residual = uv - q2[j];
                    temp_err += residual.transpose() * residual;
                }
            }

            double rho = (currentEro - temp_err) / scale;
            if (rho > 0 && std::isfinite(temp_err)) // last step was good
            {
                double alpha = 1.0 - pow((2 * rho - 1), 3);
                alpha = std::min(alpha, 2. / 3.);
                double scaleFactor = (std::max)(1. / 3., alpha);
                currentLambda *= scaleFactor;
                ni = 2;
                currentEro = temp_err;
                oneStepSuccess = true;
            }
            else
            {
                currentLambda *= ni;
                ni *= 2;
                oneStepSuccess = false;
            }

			if(!oneStepSuccess)
            {
				depth -= delta[0];
				false_cnt++;
            }
            else
            {
                false_cnt = 0;
            }
		}
		iter++;
		if (sqrt(currentEro) <= stopThresholdLM)
			stop = true;
	}

    for (int i = 0; i < 4; i++)
    {
        Points3d[i] = C0 + depth *  Ray[i];
    }

    signBatch.geometry3d = Points3d;
    signBatch.SignVecs.swap(valid_batches);
    Dump(signBatch, camera);
    return std::vector<Eigen::Vector3d>();
}

std::vector<Eigen::Vector3d> SignVec::DoWork(SignBatch& signBatch, std::map<std::string, Pose>& poseMap, const Camera& camera)
{
    std::vector<SignVecMeta> valid_batches;
    for(SignVecMeta vec : signBatch.SignVecs)
    {
        if(poseMap.find(vec.imagename) != poseMap.end())
        {
          vec.pose = poseMap[vec.imagename];
          valid_batches.push_back(vec);
        }
    }

    if(valid_batches.size() < 3)
    {
        std::cout << "the valid batches is too small to vectorizalize" << std::endl;
        return std::vector<Eigen::Vector3d>();
    }

    undistortbBox(valid_batches, camera);
    
    Eigen::Matrix3d cRb = camera.mount.rotation.matrix();
    Eigen::Vector3d cTb = camera.mount.translation;

    for(SignVecMeta& batch : valid_batches)
    {
        Eigen::Matrix3d bRw = batch.pose.orientation.toRotationMatrix();
        Eigen::Vector3d bTw = batch.pose.position;

        Eigen::Vector3d cent2d;
		cent2d.x() = batch.dlSign.bBox.GetCenter().x();
		cent2d.y() = batch.dlSign.bBox.GetCenter().y();
        cent2d.z() = 1.0;

        Eigen::Matrix3d R = bRw * cRb;
        Eigen::Vector3d C = bTw + bRw * cTb;

        batch.cent_pos = C;
        batch.cent_ray = R * camera.K.inverse() * cent2d;
        batch.cent_ray.normalize();
    }

    std::vector<ImagePair> angleBetweenImages = sortViewsForBaseline(valid_batches);
    std::sort(angleBetweenImages.begin(), angleBetweenImages.end(), [=](const ImagePair& lhs, const ImagePair& rhs) -> bool
    {
        return lhs.cosAngle < rhs.cosAngle;
    });

    ImagePair init_pair = angleBetweenImages[0];
    if(init_pair.cosAngle < 1.0)
    {
        std::cout << "the angle is too small to vectorizalize!" << std::endl;
    }

    Eigen::Vector3d vec3d;
    triangulateView(valid_batches[init_pair.left], valid_batches[init_pair.right], camera, vec3d);
    double depth = calculateDepth(valid_batches[0].pose, camera, vec3d);
    refineDepth(valid_batches, camera, depth);
    std::vector<Eigen::Vector3d> points3d = reprojectByDepth(valid_batches[0], camera, depth);

    signBatch.geometry3d = points3d;
    signBatch.SignVecs.swap(valid_batches);
    Dump(signBatch, camera);
    return std::vector<Eigen::Vector3d>();
}

std::vector<ImagePair> SignVec::sortViewsForBaseline(const std::vector<SignVecMeta>& to_vecs) const
{
    std::vector<ImagePair> matchesSizes;
    const std::size_t numImages = to_vecs.size();
    for (std::size_t i = 0; i < numImages - 1; i++) 
    {
        for (std::size_t j = i + 1; j < numImages; j++)
        {
            ImagePair pair;
            pair.left = i;
            pair.right = j;
            pair.cosAngle = to_vecs[i].cent_ray.dot(to_vecs[j].cent_ray);
            matchesSizes.push_back(pair);
            std::cout << "i = " << i << " , j = " << j << " , cosAngle = " << pair.cosAngle << std::endl;
        }
    }
    return matchesSizes;
}

//https://zhuanlan.zhihu.com/p/63179478
bool SignVec::triangulateView(const SignVecMeta& to_vec1, const SignVecMeta& to_vec2, const Camera& camera, Eigen::Vector3d& vec3d)
{
    Eigen::Matrix3d cRb = camera.mount.rotation.matrix();
    Eigen::Vector3d cTb = camera.mount.translation;

    Eigen::Matrix3d bRw1 = to_vec1.pose.orientation.toRotationMatrix();
    Eigen::Vector3d bTw1 = to_vec1.pose.position;
    Eigen::Matrix3d R1 = bRw1 * cRb;
    Eigen::Vector3d C1 = bTw1 + bRw1 * cTb;
    Eigen::Matrix<double, 3, 4> P1 = Eigen::Matrix<double, 3, 4>::Zero();
    P1.block(0, 0, 3, 3) = R1.transpose();
    P1.col(3) = - R1.transpose() * C1; 
    P1 = camera.K * P1;

    Eigen::Matrix3d bRw2 = to_vec2.pose.orientation.toRotationMatrix();
    Eigen::Vector3d bTw2 = to_vec2.pose.position;
    Eigen::Matrix3d R2 = bRw2 * cRb;
    Eigen::Vector3d C2 = bTw2 + bRw2 * cTb;
    Eigen::Matrix<double, 3, 4> P2 = Eigen::Matrix<double, 3, 4>::Zero();
    P2.block(0, 0, 3, 3) = R2.transpose();
    P2.col(3) = - R2.transpose() * C2; 
    P2 = camera.K * P2;

    const Eigen::Matrix<double, 1, 4>& P11 = P1.block(0, 0, 1, 4);
	const Eigen::Matrix<double, 1, 4>& P12 = P1.block(1, 0, 1, 4);
	const Eigen::Matrix<double, 1, 4>& P13 = P1.block(2, 0, 1, 4);
	
	const Eigen::Matrix<double, 1, 4>& P21 = P2.block(0, 0, 1, 4);
	const Eigen::Matrix<double, 1, 4>& P22 = P2.block(1, 0, 1, 4);
	const Eigen::Matrix<double, 1, 4>& P23 = P2.block(2, 0, 1, 4);

    Eigen::Vector2d cent2d1 = to_vec1.dlSign.bBox.GetCenter();
    Eigen::Vector2d cent2d2 = to_vec2.dlSign.bBox.GetCenter();

    const double &u1 = cent2d1.x();
    const double &v1 = cent2d1.y();
    const double &u2 = cent2d2.x();
    const double &v2 = cent2d2.y();

    Eigen::Matrix4d H;
    H.block(0, 0, 1, 4) = v1 * P13 - P12;
    H.block(1, 0, 1, 4) = P11 - u1 * P13;
    H.block(2, 0, 1, 4) = v2 * P23 - P22;
    H.block(3, 0, 1, 4) = P21 - u2 * P23;

    // SVD
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix4d V = svd.matrixV();

    Eigen::Vector4d X = V.block(0, 3, 4, 1);
    X = X / X(3, 0);
    vec3d = X.block(0, 0, 3, 1);
    return true;
}

void SignVec::refineVertexs(const std::vector<SignVecMeta>& to_vecs, const Camera& camera,  std::vector<Eigen::Vector3d>& vec3ds)
{
    auto robustWeightCauchy = [=](double norm_res) -> double {
		return std::sqrt(1.0 / (1.0 + norm_res * norm_res));
	};

    int N = to_vecs.size();
    std::vector<std::vector<Eigen::Vector2d>> vertexs(N);
    for (int i = 0; i < N; i++)
    {
        envelope2D bBox = to_vecs[i].dlSign.bBox;
        Eigen::Vector2d min_env, max_env;
        bBox.GetRange(min_env, max_env);

        vertexs[i].resize(4);
        vertexs[i][0] = Eigen::Vector2d(min_env.x(), min_env.y());
        vertexs[i][1] = Eigen::Vector2d(max_env.x(), min_env.y());
        vertexs[i][2] = Eigen::Vector2d(max_env.x(), max_env.y());
        vertexs[i][3] = Eigen::Vector2d(min_env.x(), max_env.y());
    }

    Eigen::Matrix3d cRb = camera.mount.rotation.matrix();
    Eigen::Vector3d cTb = camera.mount.translation;

    for(std::size_t k = 0; k < 4; k++)
    {
        Eigen::Vector3d& vec3d = vec3ds[k];
        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 3);
	    Eigen::MatrixXd g = Eigen::MatrixXd::Zero(3, 1);
        
        int iter = 0;
        while (iter < 40)
        {
            for (int i = 0; i < N; i++)
            {
                Eigen::Matrix3d bRw = to_vecs[i].pose.orientation.toRotationMatrix();
                Eigen::Vector3d bTw = to_vecs[i].pose.position;
                Eigen::Matrix3d R = bRw * cRb;
                Eigen::Vector3d C = bTw + bRw * cTb;
                Eigen::Vector3d c3d = R.transpose() * (vec3d - C);
                Eigen::Vector2d uv;
			    Utility::Project3d_2d(c3d, uv, camera.K);

                Eigen::Vector2d vextex2d = vertexs[i][k];
                Eigen::Vector2d err_i = vextex2d - uv;
                double err_i_norm = err_i.norm();
                double weight = robustWeightCauchy(err_i_norm);

                Eigen::Matrix<double, 2, 3> DeriveaUV;
                DeriveaUV << camera.fx() / c3d.z(), 0.0, - camera.fx() * c3d.x() / (c3d.z() * c3d.z()),
                    0.0, camera.fy() / c3d.z(), - camera.fy() * c3d.y() / (c3d.z() * c3d.z());

                Eigen::Matrix<double, 2, 3> J_aux = DeriveaUV * R.transpose();
                H += J_aux.transpose() * J_aux * weight;
                g += J_aux.transpose() * err_i * weight;
            }
            Eigen::Vector3d delta = H.colPivHouseholderQr().solve(g);
            double e = delta.norm();
            if (delta.norm() < EPS)
            {
                break;
            }
            vec3d.x() += delta[0];
            vec3d.y() += delta[1];
            vec3d.z() += delta[2];
            iter++;
        }
    }
}

void SignVec::undistortbBox(std::vector<SignVecMeta> &signBatch, const Camera &camera)
{
    for(SignVecMeta& sign : signBatch)
    {
        envelope2D bBox = sign.dlSign.bBox;
        Eigen::Vector2d min_env, max_env;
        bBox.GetRange(min_env, max_env);

        cv::Point2d p1 = camera.Cam2Pixel(camera.RemoveDisto(camera.Pixel2Cam(cv::Point2d(min_env.x(), min_env.y()))));
        cv::Point2d p2 = camera.Cam2Pixel(camera.RemoveDisto(camera.Pixel2Cam(cv::Point2d(max_env.x(), max_env.y()))));
        sign.dlSign.bBox = envelope2D(p1.x, p1.y, p2.x, p2.y);       
    }
}

double SignVec::calculateDepth(const Pose& pose, const Camera& camera, Eigen::Vector3d& vec3d) const
{
    Eigen::Matrix3d cRb = camera.mount.rotation.matrix();
    Eigen::Vector3d cTb = camera.mount.translation;

    Eigen::Matrix3d bRw0 = pose.orientation.toRotationMatrix();
    Eigen::Vector3d bTw0 = pose.position;
    Eigen::Matrix3d R0 = bRw0 * cRb;
    Eigen::Vector3d C0 = bTw0 + bRw0 * cTb;

    Eigen::Vector3d camera3D = R0.transpose() * (vec3d - C0);
    return camera3D.z();
}

std::vector<Eigen::Vector3d> SignVec::reprojectByDepth(const SignVecMeta& signVec, const Camera& camera, double depth) const
{
    Eigen::Matrix3d cRb = camera.mount.rotation.matrix();
    Eigen::Vector3d cTb = camera.mount.translation;

    std::vector<Eigen::Vector2d> q1(4), q2(4);
    q1[0] = Eigen::Vector2d(signVec.dlSign.bBox.GetMinx(),  signVec.dlSign.bBox.GetMiny());
    q1[1] = Eigen::Vector2d(signVec.dlSign.bBox.GetMinx() + signVec.dlSign.bBox.Width(), signVec.dlSign.bBox.GetMiny());
    q1[2] = Eigen::Vector2d(signVec.dlSign.bBox.GetMinx() + signVec.dlSign.bBox.Width(), signVec.dlSign.bBox.GetMiny() + signVec.dlSign.bBox.Height());
    q1[3] = Eigen::Vector2d(signVec.dlSign.bBox.GetMinx(),  signVec.dlSign.bBox.GetMiny() + signVec.dlSign.bBox.Height());

    Eigen::Matrix3d bRw0 = signVec.pose.orientation.toRotationMatrix();
    Eigen::Vector3d bTw0 = signVec.pose.position;
    Eigen::Matrix3d R0 = bRw0 * cRb;
    Eigen::Vector3d C0 = bTw0 + bRw0 * cTb;

    std::vector<Eigen::Vector3d> Points3d(4), Ray(4);
    for (int i = 0; i < 4; i++)
    {
        Ray[i] = cRb * camera.K.inverse() * Eigen::Vector3d(q1[i].x(), q1[i].y(), 1.0);
        Ray[i] = Ray[i] / Ray[i].x();
        Ray[i] = bRw0 * Ray[i];
        Ray[i].normalize();
        Points3d[i] = C0 + depth *  Ray[i];
    }
    return Points3d;
}

bool SignVec::refineDepth(const std::vector<SignVecMeta> &to_vecs, const Camera &camera, double &depth) const
{
    Eigen::Matrix3d cRb = camera.mount.rotation.matrix();
    Eigen::Vector3d cTb = camera.mount.translation;

    int N = to_vecs.size();
    std::vector<Eigen::Vector2d> q1(4), q2(4);
    q1[0] = Eigen::Vector2d(to_vecs[0].dlSign.bBox.GetMinx(),  to_vecs[0].dlSign.bBox.GetMiny());
    q1[1] = Eigen::Vector2d(to_vecs[0].dlSign.bBox.GetMinx() + to_vecs[0].dlSign.bBox.Width(), to_vecs[0].dlSign.bBox.GetMiny());
    q1[2] = Eigen::Vector2d(to_vecs[0].dlSign.bBox.GetMinx() + to_vecs[0].dlSign.bBox.Width(), to_vecs[0].dlSign.bBox.GetMiny() + to_vecs[0].dlSign.bBox.Height());
    q1[3] = Eigen::Vector2d(to_vecs[0].dlSign.bBox.GetMinx(),  to_vecs[0].dlSign.bBox.GetMiny() + to_vecs[0].dlSign.bBox.Height());

    Eigen::Matrix3d bRw0 = to_vecs[0].pose.orientation.toRotationMatrix();
    Eigen::Vector3d bTw0 = to_vecs[0].pose.position;
    Eigen::Matrix3d R0 = bRw0 * cRb;
    Eigen::Vector3d C0 = bTw0 + bRw0 * cTb;

    std::vector<Eigen::Vector3d> Points3d(4), Ray(4);
    for (int i = 0; i < 4; i++)
    {
        Ray[i] = cRb * camera.K.inverse() * Eigen::Vector3d(q1[i].x(), q1[i].y(), 1.0);
        Ray[i] = Ray[i] / Ray[i].x();
        Ray[i] = bRw0 * Ray[i];
        Ray[i].normalize();
        Points3d[i] = C0 + depth *  Ray[i];
    }

    double stopThresholdLM = 0.0;
    double currentEro = 0.0;
    double currentLambda = 0.0;
    double ni = 2.0;

    //Generate init lamda
	{
        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 1);
        for(int i = 1; i < N; i++)
        {
            q2[0] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx(), to_vecs[i].dlSign.bBox.GetMiny());
            q2[1] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx() + to_vecs[i].dlSign.bBox.Width(), to_vecs[i].dlSign.bBox.GetMiny());
            q2[2] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx() + to_vecs[i].dlSign.bBox.Width(), to_vecs[i].dlSign.bBox.GetMiny() + to_vecs[i].dlSign.bBox.Height());
            q2[3] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx(), to_vecs[i].dlSign.bBox.GetMiny() + to_vecs[i].dlSign.bBox.Height());

            Eigen::Matrix3d bRw = to_vecs[i].pose.orientation.toRotationMatrix();
            Eigen::Vector3d bTw = to_vecs[i].pose.position;
            Eigen::Matrix3d R = bRw * cRb;
            Eigen::Vector3d C = bTw + bRw * cTb;

            for (int j = 0; j < 4; j++)
            {
                Eigen::Vector3d camera3D = R.transpose() * (Points3d[j] - C);
                if (camera3D[2] <= 0.0)
                    continue;

                Eigen::Vector3d uvw = camera.K * camera3D;
                Eigen::Vector2d uv(uvw[0] / uvw[2], uvw[1] / uvw[2]);
                Eigen::Vector2d residual = uv - q2[j];
                currentEro += residual.transpose() * residual;

                Eigen::Matrix<double, 2, 3> J_F;
                J_F << camera.fx() / camera3D.z(), 0.0, -camera.fx() * camera3D.x() / (camera3D.z() * camera3D.z()),
                    0.0, camera.fy() / camera3D.z(), -camera.fy() * camera3D.y() / (camera3D.z() * camera3D.z());
                Eigen::Matrix<double, 2, 1> J_aux = J_F * R.transpose() * Ray[j];
			    H += J_aux.transpose() * J_aux;
            }
		}

        stopThresholdLM = 1e-6 * currentEro;
        double maxDiagonal = std::max(std::fabs(H(0, 0)), maxDiagonal);
        double tau = 1e-5;
        currentLambda = tau * maxDiagonal;
    }

    auto robustWeightCauchy = [=](double norm_res) -> double {
		return std::sqrt(1.0 / (1.0 + norm_res * norm_res));
	};

	int iter = 0;
	bool stop = false;
	while (!stop && iter < 30)
	{
		bool oneStepSuccess = false;
        int false_cnt = 0;

		while (!oneStepSuccess)
		{
            Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 1);
            Eigen::MatrixXd g = Eigen::MatrixXd::Zero(1, 1);

            for (int i = 1; i < N; i++)
            {
                Points3d[0] = C0 + depth * Ray[0];
                Points3d[1] = C0 + depth * Ray[1];
                Points3d[2] = C0 + depth * Ray[2];
                Points3d[3] = C0 + depth * Ray[3];

                q2[0] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx(),  to_vecs[i].dlSign.bBox.GetMiny());
                q2[1] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx() + to_vecs[i].dlSign.bBox.Width(), to_vecs[i].dlSign.bBox.GetMiny());
                q2[2] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx() + to_vecs[i].dlSign.bBox.Width(), to_vecs[i].dlSign.bBox.GetMiny() + to_vecs[i].dlSign.bBox.Height());
                q2[3] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx(),  to_vecs[i].dlSign.bBox.GetMiny() + to_vecs[i].dlSign.bBox.Height());

                Eigen::Matrix3d bRw = to_vecs[i].pose.orientation.toRotationMatrix();
                Eigen::Vector3d bTw = to_vecs[i].pose.position;
                Eigen::Matrix3d R = bRw * cRb;
                Eigen::Vector3d C = bTw + bRw * cTb;

                for (int j = 0; j < 4; j++)
                {
                    Eigen::Vector3d camera3D = R.transpose() * (Points3d[j] - C);
                    if (camera3D[2] <= 0.0)
                        continue;

                    Eigen::Vector3d uvw = camera.K * camera3D;
                    Eigen::Vector2d uv(uvw[0] / uvw[2], uvw[1] / uvw[2]);
                    Eigen::Vector2d residual = uv - q2[j];
                    
                    double err_i_norm = residual.norm();
				    double weight = robustWeightCauchy(err_i_norm);

                    Eigen::Matrix<double, 2, 3> J_F;
                    J_F << camera.fx() / camera3D.z(), 0.0, -camera.fx() * camera3D.x() / (camera3D.z() * camera3D.z()),
                        0.0, camera.fy() / camera3D.z(), -camera.fy() * camera3D.y() / (camera3D.z() * camera3D.z());
                    Eigen::Matrix<double, 2, 1> J_aux = J_F * R.transpose() * Ray[j];
                    
                    H += J_aux.transpose() * J_aux * weight;
                    g -= J_aux.transpose() * residual * weight;
                }
            }

            H(0, 0) += currentLambda;

			Eigen::Matrix<double, 1, 1> delta = H.colPivHouseholderQr().solve(g);
            if (delta.squaredNorm() < EPS || false_cnt > 10)
            {
                stop = true;
                break;
            }

			depth += delta[0];

			double scale = 0;
            scale = delta.transpose() * (currentLambda * delta + g);
            scale += 1e-3;

			double temp_err = 0.0;
            for (int i = 1; i < N; i++)
            {
                Points3d[0] = C0 + depth * Ray[0];
                Points3d[1] = C0 + depth * Ray[1];
                Points3d[2] = C0 + depth * Ray[2];
                Points3d[3] = C0 + depth * Ray[3];

                q2[0] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx(),  to_vecs[i].dlSign.bBox.GetMiny());
                q2[1] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx() + to_vecs[i].dlSign.bBox.Width(), to_vecs[i].dlSign.bBox.GetMiny());
                q2[2] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx() + to_vecs[i].dlSign.bBox.Width(), to_vecs[i].dlSign.bBox.GetMiny() + to_vecs[i].dlSign.bBox.Height());
                q2[3] = Eigen::Vector2d(to_vecs[i].dlSign.bBox.GetMinx(),  to_vecs[i].dlSign.bBox.GetMiny() + to_vecs[i].dlSign.bBox.Height());

                Eigen::Matrix3d bRw = to_vecs[i].pose.orientation.toRotationMatrix();
                Eigen::Vector3d bTw = to_vecs[i].pose.position;
                Eigen::Matrix3d R = bRw * cRb;
                Eigen::Vector3d C = bTw + bRw * cTb;

                for (int j = 0; j < 4; j++)
                {
                    Eigen::Vector3d camera3D = R.transpose() * (Points3d[j] - C);
                    if (camera3D[2] <= 0.0)
                        continue;

                    Eigen::Vector3d uvw = camera.K * camera3D;
                    Eigen::Vector2d uv(uvw[0] / uvw[2], uvw[1] / uvw[2]);
                    Eigen::Vector2d residual = uv - q2[j];
                    temp_err += residual.transpose() * residual;
                }
            }

            double rho = (currentEro - temp_err) / scale;
            if (rho > 0 && std::isfinite(temp_err)) // last step was good
            {
                double alpha = 1.0 - pow((2 * rho - 1), 3);
                alpha = std::min(alpha, 2. / 3.);
                double scaleFactor = (std::max)(1. / 3., alpha);
                currentLambda *= scaleFactor;
                ni = 2;
                currentEro = temp_err;
                oneStepSuccess = true;
            }
            else
            {
                currentLambda *= ni;
                ni *= 2;
                oneStepSuccess = false;
            }

			if(!oneStepSuccess)
            {
				depth -= delta[0];
				false_cnt++;
            }
            else
            {
                false_cnt = 0;
            }
		}
		iter++;
		if (sqrt(currentEro) <= stopThresholdLM)
			stop = true;
	}
    return depth;
}

void SignVec::Dump(const SignBatch &signBatch, const Camera &camera)
{
    Eigen::Matrix3d cRb = camera.mount.rotation.matrix();
    Eigen::Vector3d cTb = camera.mount.translation;

    std::stringstream path_ss;
    path_ss << "./vectorization/" << signBatch.id;
    FileUtils::folder_create(path_ss.str());

    for(SignVecMeta batch : signBatch.SignVecs)
    {
        cv::Mat image = cv::imread(image_root_ + "/" + batch.imagename), undistrot_img;
        cv::undistort(image, undistrot_img, camera.get_K(), camera.get_D(), camera.get_K());

        Eigen::Matrix3d bRw = batch.pose.orientation.toRotationMatrix();
        Eigen::Vector3d bTw = batch.pose.position;

        for(Eigen::Vector3d vertex3d : signBatch.geometry3d)
        {
            Eigen::Vector3d c3d = cRb.transpose() * (bRw.transpose() * (vertex3d - bTw) - cTb);
            Eigen::Vector2d uv;
			if (Utility::Project3d_2d(c3d, uv, camera.K))
			{
                cv::circle(undistrot_img, cv::Point(uv.x(), uv.y()), 2.0, cv::Scalar(0, 0, 255), 2);       
            }
        }

        std::string save_path = path_ss.str() + "/" + batch.imagename;
        cv::imwrite(save_path, undistrot_img);
    }

    std::string obj_path = path_ss.str() + ".obj";
    std::ofstream file(obj_path);
    file.precision(10);

    for(std::size_t i = 0; i < signBatch.geometry3d.size(); i++)
    {
        file << "v " << signBatch.geometry3d[i].x() << " " << signBatch.geometry3d[i].y() << " " << signBatch.geometry3d[i].z() << std::endl;
    }
    file << "v " << signBatch.geometry3d[0].x() << " " << signBatch.geometry3d[0].y() << " " << signBatch.geometry3d[0].z() << std::endl;

    for(std::size_t i = 0; i < signBatch.geometry3d.size(); i++)
    {
        file << "l " << i + 1 << " " << i + 2 << std::endl;
    }

    file.close();
}